﻿#include "f4cagrinstrument.h"
#define  CodeWord    1 / 32768
#include <qmath.h>
#include<QDir>
#include<QSettings>
#include<QDebug>
F4CAgrInstrument::F4CAgrInstrument()
{
    QDir settingDir(QDir::currentPath());
    settingDir.cdUp();
    settingDir.setPath(settingDir.absolutePath().append("/config/FY4TaskOptimization"));

    if(!settingDir.exists())
        settingDir.mkdir(settingDir.absolutePath());

    QSettings settings(settingDir.absoluteFilePath("setting.ini"),QSettings::IniFormat);
    settings.beginGroup("FY4C-Agr-Mirrors");

    if(settings.contains("MirrorHeight"))
        mdMirrorHeight=settings.value("MirrorHeight").toDouble();
    else
    {
        mdMirrorHeight=14336*0.5*180.0/ M_PI*1E-6;
        settings.setValue("MirrorHeight",QString::number(mdMirrorHeight,'f',6));

    }
    if(settings.contains("MirrorLeftHalf"))
        mdMirrorLeftHalf=settings.value("MirrorLeftHalf").toDouble();
    else
    {
        mdMirrorLeftHalf=2268*0.5*180.0/M_PI*1E-6;
        settings.setValue("MirrorLeftHalf",QString::number(mdMirrorLeftHalf,'f',6));

    }
    if(settings.contains("MirrorRightHalf"))
        mdMirrorRightHalf=settings.value("MirrorRightHalf").toDouble();
    else
    {
        mdMirrorRightHalf=2604*0.5*180.0/M_PI*1E-6;
        settings.setValue("MirrorRightHalf",QString::number(mdMirrorRightHalf,'f',6));
    }
    if(settings.contains("MirrorScanSpeed"))
        mdMirrorScanSpeed=settings.value("MirrorScanSpeed").toDouble();
    else
    {
        mdMirrorScanSpeed=1;
        settings.setValue("MirrorScanSpeed",QString::number(mdMirrorScanSpeed,'f',6));
    }
    if(settings.contains("FastPointSpeed"))
        mdFastPointSpeed=settings.value("FastPointSpeed").toDouble();
    else
    {
        mdFastPointSpeed=10.0;
        settings.setValue("FastPointSpeed",mdFastPointSpeed);
    }
    if(settings.contains("ScanFastPointSpeed"))
        mdScanFastPointSpeed=settings.value("ScanFastPointSpeed").toDouble();
    else
    {
        mdScanFastPointSpeed=0.78;
        settings.setValue("ScanFastPointSpeed",mdScanFastPointSpeed);
    }
    settings.endGroup();
    settings.beginGroup("FY4C-Agr-RegionCoordinates");
    if(settings.contains("FulldiskNormalRegionEW"))
    {
        QString pos=settings.value("FulldiskNormalRegionEW").toString();
        //mdFulldisEWMin=pos.section(",",0,0).toDouble();
        //mdFulldisEWMax=pos.section(",",0,1).toDouble();
        mdFulldisEWMin=pos.split(",").at(0).toDouble();
        mdFulldisEWMax=pos.split(",").at(1).toDouble();
    }
    else
    {
        mdFulldisEWMax=4.35;
        mdFulldisEWMin=-4.35;
        settings.setValue("FulldiskNormalRegionEW",QString("%1,%2").arg(mdFulldisEWMin).arg(mdFulldisEWMax));
    }
    if(settings.contains("FulldiskNormalRegionNS"))
    {
        QString pos=settings.value("FulldiskNormalRegionNS").toString();
        mdFulldisNSMin=pos.split(",").at(0).toDouble();
        mdFulldisNSMax=pos.split(",").at(1).toDouble();
    }
    else
    {
        mdFulldisNSMax=4.35;
        mdFulldisNSMin=-4.35;
        settings.setValue("FulldiskNormalRegionNS",QString("%1,%2").arg(mdFulldisNSMin).arg(mdFulldisNSMax));
    }
    if(settings.contains("ChinascanNormalRegionEW"))
    {
        QString pos=settings.value("ChinascanNormalRegionEW").toString();
        mdChinaNormalEWMin=pos.split(",").at(0).toDouble();
        mdChinaNormalEWMax=pos.split(",").at(1).toDouble();
    }
    else
    {
        mdChinaNormalEWMax=3.399400;
        mdChinaNormalEWMin=-2.778170;
        settings.setValue("ChinascanNormalRegionEW",QString("%1,%2").arg(mdChinaNormalEWMin).arg(mdChinaNormalEWMax));
    }
    if(settings.contains("ChinascanNormalRegionNS"))
    {
        QString pos=settings.value("ChinascanNormalRegionNS").toString();
        mdChinaNormalNSMin=pos.split(",").at(0).toDouble();
        mdChinaNormalNSMax=pos.split(",").at(1).toDouble();
    }
    else
    {
        mdChinaNormalNSMax=3.825990;
        mdChinaNormalNSMin=0.410156;
        settings.setValue("ChinascanNormalRegionNS",QString("%1,%2").arg(mdChinaNormalNSMin).arg(mdChinaNormalNSMax));
    }
    settings.endGroup();
}

QStringList F4CAgrInstrument::taskTypes()
{
    QStringList tasks;
    tasks.clear();

    tasks.append(QString("FulldiskNormal"));
    tasks.append(QString("InfraredBackground"));
    tasks.append(QString("BlackbodyScan"));
    tasks.append(QString("StarScan"));
    tasks.append(QString("ChinascanNormal"));
    tasks.append(QString("sPaceScan"));
    //tasks.append(QString("MoonScan"));
    //tasks.append(QString("RegionScan"));
    tasks.append(QString("MidRegionScan"));

    return tasks;
}

QString F4CAgrInstrument::instrumentName()
{
    return "FY4C-Agr";
}
int F4CAgrInstrument::calCommandTime(CommandItem *pCommand)
{

    QDir settingDir(QDir::currentPath());
    settingDir.cdUp();
    settingDir.cd("config/FY4TaskOptimization");
    QSettings setting(settingDir.absoluteFilePath("setting.ini"),QSettings::IniFormat);

    double Pos_NSMove = qAbs(pCommand->getValue(ScriptItem::Pos_NSEnd) - pCommand->getValue(ScriptItem::Pos_NSStart));
    double Pos_EWMove = qAbs(pCommand->getValue(ScriptItem::Pos_EWEnd) - pCommand->getValue(ScriptItem::Pos_EWEnd));

    double Pos_SEMove =  Pos_NSMove > Pos_EWMove ? Pos_NSMove : Pos_EWMove;
    double ScanTurnDis = 0.0;                                               //扫描镜转弯长度
    //快速指向
    if(pCommand->getWorkMode() == CommandItem::Mode_FastPoint)
    {
        if((Pos_SEMove / CodeWord) <= 7217)
        {
            pCommand->setValue(ScriptItem::Time_End,
                               (setting.value("FY4C-Agr/ShortGrearTime").toDouble())+ pCommand->getValue(ScriptItem::Time_Start));
            if(!pCommand->getValue(ScriptItem::Time_End))
            {
                return 11;
            }
        }
        else if((Pos_SEMove / CodeWord) <= 163802)
        {
            pCommand->setValue(ScriptItem::Time_End,
                               (setting.value("FY4C-Agr/MeanGrearTime").toDouble())+ pCommand->getValue(ScriptItem::Time_Start));
            if(!pCommand->getValue(ScriptItem::Time_End))
            {
                return 12;
            }
        }
        else
        {
            pCommand->setValue(ScriptItem::Time_End,
                               (setting.value("FY4C-Agr/MeanGrearTime").toDouble())+
                               ceil(((Pos_SEMove / CodeWord)- 163802) / 1638)*0.005 +
                               pCommand->getValue(ScriptItem::Time_Start));
            if(!pCommand->getValue(ScriptItem::Time_End))
            {
                return 13;
            }
        }
    }
    //扫描
    else if(pCommand->getWorkMode() == CommandItem::Mode_SlideScan)
    {
        ScanTurnDis = setting.value("FY4C-Agr/ScanSpeed").toDouble() / 10;
        pCommand->setValue(ScriptItem::Time_End,
                           ceil((Pos_SEMove - ScanTurnDis)/setting.value("FY4C-Agr/ScanSpeed").toDouble() * 200) * 0.005 +
                           setting.value("FY4C-Agr/ScanGrearTime").toDouble()+
                           pCommand->getValue(ScriptItem::Time_Start));
        if(!pCommand->getValue(ScriptItem::Time_End))
        {
            return 21;
        }
    }
    else{
        return 999;
    }
    return 0;
}

int F4CAgrInstrument::calCurrentPos(CommandItem *pCommand, double dTime, double &dPosEW, double &dPosNS)
{
    QDir settingDir(QDir::currentPath());
    settingDir.cdUp();
    settingDir.cd("config/FY4TaskOptimization");
    QSettings setting(settingDir.absoluteFilePath("setting.ini"),QSettings::IniFormat);
    if(dTime > (pCommand->getValue(ScriptItem::Time_End)- pCommand->getValue(ScriptItem::Time_Start)))
    {
        return 22;
    }
    //快速指向
    if(pCommand->getWorkMode() == CommandItem::Mode_FastPoint)
    {
        double pos[2];
        pos[0] = abs(pCommand->getValue(ScriptItem::Pos_EWStart) - pCommand->getValue(ScriptItem::Pos_EWEnd))/CodeWord;
        pos[1] = abs(pCommand->getValue(ScriptItem::Pos_NSStart) - pCommand->getValue(ScriptItem::Pos_NSEnd))/CodeWord;
        for(int i = 0 ; i < 2 ; i++)
        {
            double nomain;
            if(pos[i] <= 7217)
            {
                nomain = 5 * pos[i]*(dTime - sin(10 * M_PI * dTime) / 10 / M_PI);
            }
            else if(pos[i] <= 163802)
            {
                nomain = pos[i]*(dTime - sin(2 * M_PI * dTime) / 2 / M_PI);
            }
            else
            {
                if(dTime <= (setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2))
                {
                    nomain = 5 * (dTime - sin(2 * M_PI * dTime) / 2 /M_PI);
                }
                else if(dTime <= pos[i]/10)
                {
                    nomain = 5 * ((setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2) -
                                  sin(2 * M_PI * (setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2))
                                  / 2 / M_PI) + 10 * (dTime - setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2);
                }
                else
                {
                    nomain = 5 * ((setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2) -
                                  sin(2 * M_PI * (setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2))
                                  / 2 / M_PI) + 10 * pos[i] / 10 +
                            (5 * ((setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2) -
                                  sin(2 * M_PI * (setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2))
                                  / 2 / M_PI))-
                            (5 * ((setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2 - dTime + pos[i] / 10) -
                                  sin(2 * M_PI * (setting.value("FY4C-Agr/MeanGrearTime").toDouble()/2 - dTime + pos[i] / 1))/ 2 / M_PI));

                }
            }
            if(i==0)
            {
                dPosEW = nomain;
                if(!dPosEW)
                {
                    return 31;
                }
            }
            else
            {
                dPosNS = nomain;
                if(!dPosNS)
                {
                    return 32;
                }
            }
        }
    }
    //扫描
    else if(pCommand->getWorkMode() == CommandItem::Mode_SlideScan)
    {
        dPosNS = pCommand->getValue(ScriptItem::Pos_NSStart);
        if(!dPosNS)
        {
            return 41;
        }
        double posdis= abs(pCommand->getValue(ScriptItem::Pos_EWStart) - pCommand->getValue(ScriptItem::Pos_EWEnd)) * CodeWord;
        if(dTime <= (setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2))
        {
            dPosEW = (setting.value("FY4C-Agr/ScanSpeed").toDouble() * CodeWord)/2 *
                    (dTime - sin(10 * M_PI * dTime)/ 10 / M_PI);
            if(!dPosEW)
            {
                return 51;
            }
        }
        else if(dTime <= posdis / 10)
        {
            dPosEW = (setting.value("FY4C-Agr/ScanSpeed").toDouble() * CodeWord)/2 *
                    ((setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2) - sin(10 * M_PI *
                                                                                  (setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2))/ 10 / M_PI) +
                    (setting.value("FY4C-Agr/ScanSpeed").toDouble() * CodeWord) * (dTime -
                                                                                   (setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2));
            if(!dPosEW)
            {
                return 52;
            }
        }
        else
        {
            dPosEW = (setting.value("FY4C-Agr/ScanSpeed").toDouble() * CodeWord)/2 *
                    ((setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2) - sin(10 * M_PI *
                                                                                  (setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2))/ 10 / M_PI) +
                    (setting.value("FY4C-Agr/ScanSpeed").toDouble() * CodeWord) * posdis / 10 +
                    ((setting.value("FY4C-Agr/ScanSpeed").toDouble() * CodeWord)/2 *
                     ((setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2) - sin(10 * M_PI *
                                                                                   (setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2))/ 10 / M_PI)) -
                    (setting.value("FY4C-Agr/ScanSpeed").toDouble() * CodeWord)/2 *
                    ((setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2 - dTime + posdis / 10) - sin(10 * M_PI *
                                                                                                        (setting.value("FY4C-Agr/ScanGrearTime").toDouble()/2))/ 10 / M_PI);
            if(!dPosEW)
            {
                return 53;
            }
        }
    }
    else
    {
        return 999;
    }
    return 0;
}
void F4CAgrInstrument::getActuallocation(double x,double dx,double rlx[],double v){
#if 1
    double Theoryx = qCeil((dx + 0.05 + mdMirrorRightHalf  - (x - 0.05 - 0.2 - mdMirrorLeftHalf)) * 32768);//理论扫描一行距离取整操作
    double startx,endx;
    if(fmod(Theoryx , 164) > 0)
    {
        double UniSpeedx = qCeil(( Theoryx - 164 * v * (20 + 20) / 2)/(164 * v) )*(164 * v);//匀速段做补偿操作
        double FaSlx = (Theoryx - UniSpeedx) / 2;  //补偿操作后加减速段的距离
        double pPlus = (Theoryx  - (dx -x)*32768 )-(Theoryx - UniSpeedx);  //补偿距离
        if(fmod(( pPlus ) / 2 , 2) != 0 ){
            pPlus = pPlus + 1;
        }
        startx = x - 0.2 - mdMirrorLeftHalf - pPlus / 32768 / 2 - FaSlx/32768 ;
        endx =dx + pPlus / 32768 / 2 +  mdMirrorRightHalf + FaSlx/32768 ;
    }
    rlx[0] = startx;
    rlx[1] = endx;
    return ;
#else
    rlx[0]=x - (0.5*(0.1-sin(M_PI)/(10*M_PI))) -0.2;
    rlx[1]=dx + (0.5*(0.1-sin(M_PI)/(10*M_PI)));
#endif
}

double F4CAgrInstrument::getScanTime(double x,double dx,double v){
#if 1//03星
    double mx = qAbs(dx - x)*32768;
    double t = 0.1+0.1+0.2+qCeil((mx- (164*v) * 20)/(164*v))*0.005;
    return t;
#else  //02星
    double mx=qAbs(dx-x);
    double t=0.1+0.1+(mx-(0.5*(0.1-sin(M_PI)/(10*M_PI)))*2)/1;
    return t;
#endif
}
double F4CAgrInstrument::getQuickTime(double x, double y, double dx, double dy,double v)
{
#if 1
    double mx = qAbs(dx - x);  // mx  水平位移
    double my = qAbs(dy - y);  // my垂直位移

    double dis = qMax(mx , my)  * 32768;    // 移动距离（码字）
    double t;
    double maxv_5ms=v*32768*0.005;
    if(dis <= 7217) //根据文档采用acc2
    {
        //没有匀速段
        t =0.1+0.1;
    }
    else if(dis<=1638 * 100)
    {
        //没有匀速段
        t=0.5+0.5 ;
    }
#if 0
    else
    {
        t = 0.5+0.5+qCeil((dis - 163800) / 1638) * 0.005;
    }
#else

    else
    {
        t=0.5+0.5+qCeil((dis-163800)/maxv_5ms)*0.005;
    }
#endif
    return t;
#else
    double mx = qAbs(dx - x);
    double my = qAbs(dy - y);

    double dis = sqrt(mx * mx + my * my);
    double t;
    if(dis <= (16384 * CodeWord))
    {
        t = 0.1 + 0.1 ;

    }
    else if(dis <= (163840 * CodeWord))
    {
        t = 0.5 + 0.5 ;
    }
    else
    {
        t = 0.5 + 0.5 + (dis - (5 * (0.5 - sin(M_PI)/(2 * M_PI))) * 2) / 10 ;
    }

    return t;
#endif
}

